/**
 * @file TiltReactionPlanner.h
 * @author  
 * @brief 倾角异常控制设定
 * @version 1.0
 * @date 2020-07-23
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#pragma once
#include "data_struct/base/DataSet.h"

namespace  behavior_controller
{
    class TiltReactionPlanner
    {
        public:

            TiltReactionPlanner(){};
            /**
             * @brief 上盖障碍物响应的运行函数
             * @return
             */
            void run(double &linear_vel, double &angular_vel);

            /**
             * @brief 控制机器人后退函数，退出障碍物区域
             * @return
             */
            void Back(double &linear_vel, double &angular_vel);

            /**
             * @brief 控制机器人转向函数，转到平行障碍物区域
             * @return
             */
            void Turn(double &linear_vel, double &angular_vel);
            void TurnToAngle(double &linear_vel, double &angular_vel);

            double last_angle;
            double global_goal_angle;
            int linear_vel_sig=1;
            int angular_vel_sig=1;
            bool finished = true;
            bool finished_turn= false;
    };
    //static 
    extern TiltReactionPlanner tilt_reaction_planner;
}